﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using Microsoft.Kinect;
using System.Runtime.InteropServices;

namespace HandTracking
{
    public partial class MainWindow : Window
    {
        private KinectSensor kinectSensor;

        private Point HandLeft;
        private float HandLeftDepth;
        private Point HandRight;
        private float HandRightDepth;
        private float ShoulderCenterDepth;
        private Point ElbowLeft;
        private float ElbowLeftDepth;
        private Point ElbowRight;
        private float ElbowRightDepth;

        Ellipse leftHandFullBodyEllipse;
        Ellipse rightHandFullBodyEllipse;
        Ellipse leftElbowEllipse;
        Ellipse rightElbowEllipse;

        Ellipse leftHandEllipse;
        Ellipse rightHandEllipse;

        Line leftLine;
        Line rightLine;

        public enum ImageType { Color, Depth }

        // color divisors for tinting depth pixels
        private static readonly int[] IntensityShiftByPlayerR = { 2, 1, 0, 2, 0, 0, 2, 0 };
        private static readonly int[] IntensityShiftByPlayerG = { 2, 1, 2, 0, 2, 0, 0, 1 };
        private static readonly int[] IntensityShiftByPlayerB = { 0, 1, 2, 2, 0, 2, 0, 2 };

        private const int RedIndex = 2;
        private const int GreenIndex = 1;
        private const int BlueIndex = 0;
        private static readonly int Bgr32BytesPerPixel = (PixelFormats.Bgr32.BitsPerPixel + 7) / 8;

        //Use to transform and reduce jitter
        private TransformSmoothParameters TransformParameters = new TransformSmoothParameters
        {
            Smoothing = 0.1f,
            Correction = 0.3f,
            Prediction = 0.4f,
            JitterRadius = 1.0f,
            MaxDeviationRadius = 0.5f
        };

        public MainWindow()
        {
            InitializeComponent();

            this.kinectSensor = KinectSensor.KinectSensors[0];
            try
            {
                // Pass parameters to set smoothing
                kinectSensor.SkeletonStream.Enable(TransformParameters);
                //kinectSensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
                kinectSensor.DepthStream.Enable(DepthImageFormat.Resolution320x240Fps30);
                kinectSensor.Start();
            }
            catch (InvalidOperationException)
            {
                MessageBox.Show("Runtime initialization failed. Ensure Kinect is plugged in");
                return;
            }

            //nui.SkeletonEngine.SmoothParameters = parameters;

            kinectSensor.AllFramesReady += new EventHandler<AllFramesReadyEventArgs>(KinectAllFramesReady);

            leftHandFullBodyEllipse = InitailizeEllipse();
            rightHandFullBodyEllipse = InitailizeEllipse();
            leftElbowEllipse = InitailizeEllipse();
            rightElbowEllipse = InitailizeEllipse();
            leftHandEllipse = InitailizeEllipse();
            rightHandEllipse = InitailizeEllipse();
            leftLine = InitailizeLine();
            rightLine = InitailizeLine();
        }

        private Ellipse InitailizeEllipse()
        {
            Ellipse ellipse = new Ellipse();
            ellipse.Width = 10;
            ellipse.Height = 10;
            ellipse.Fill = new SolidColorBrush(Colors.Black);
            return ellipse;
        }

        private Line InitailizeLine()
        {
            Line line = new Line();
            line.Stroke = new SolidColorBrush(Colors.Black);
            line.StrokeThickness = 3;
            return line;
        }


        Skeleton[] skeletons;

        private void KinectAllFramesReady(object sender, AllFramesReadyEventArgs e)
        {
            if (!((KinectSensor)sender).SkeletonStream.IsEnabled)
            {
                return;
            }

            using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
            {
                bool receivedData = false;

                if (skeletonFrame != null)
                {
                    if (skeletons == null) //allocate the first time
                    {
                        skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
                    }

                    skeletonFrame.CopySkeletonDataTo(skeletons);

                    receivedData = true;
                }
                else
                {
                    // apps processing of skeleton data took too long; it got more than 2 frames behind.
                    // the data is no longer avabilable.
                    receivedData = false;
                }

                if (receivedData)
                {
                    // DISPLAY OR PROCESS IMAGE DATA IN skeletons HERE

                    // Gets first skeleton (Only tracks one)
                    Skeleton skeletalData = (from s in skeletons
                                             where s != null && s.TrackingState == SkeletonTrackingState.Tracked
                                             select s).FirstOrDefault();

                    // Clear ellipses on hands and return
                    if (skeletalData == null)
                    {
                        canvas.Children.Clear();
                        leftHandCanvas.Children.Clear();
                        return;
                    }

                    if (SkeletonTrackingState.Tracked == skeletalData.TrackingState)
                    {
                        using (DepthImageFrame depthImageFrame = e.OpenDepthImageFrame())
                        {
                            if (depthImageFrame != null)
                            {
                                // Draw joints
                                foreach (Joint joint in skeletalData.Joints)
                                {
                                    switch (joint.JointType)
                                    {
                                        case JointType.HandLeft:
                                            HandLeft = GetPosition2DLocation(depthImageFrame, joint.Position);
                                            HandLeftDepth = joint.Position.Z;
                                            LeftDepthValue.Content = HandLeftDepth + " mm";
                                            Canvas.SetLeft(leftHandFullBodyEllipse, HandLeft.X - leftHandFullBodyEllipse.Width / 2);
                                            Canvas.SetTop(leftHandFullBodyEllipse, HandLeft.Y - leftHandFullBodyEllipse.Height / 2);
                                            break;
                                        case JointType.HandRight:
                                            HandRight = GetPosition2DLocation(depthImageFrame, joint.Position);
                                            HandRightDepth = joint.Position.Z;
                                            RightDepthValue.Content = HandRightDepth + " mm";
                                            Canvas.SetLeft(rightHandFullBodyEllipse, HandRight.X - rightHandFullBodyEllipse.Width / 2);
                                            Canvas.SetTop(rightHandFullBodyEllipse, HandRight.Y - rightHandFullBodyEllipse.Height / 2);
                                            break;
                                        case JointType.ShoulderCenter:
                                            ShoulderCenterDepth = joint.Position.Z;
                                            break;
                                        case JointType.ElbowLeft:
                                            ElbowLeft = GetPosition2DLocation(depthImageFrame, joint.Position);
                                            ElbowLeftDepth = joint.Position.Z;
                                            Canvas.SetLeft(leftElbowEllipse, ElbowLeft.X - leftElbowEllipse.Width / 2);
                                            Canvas.SetTop(leftElbowEllipse, ElbowLeft.Y - leftElbowEllipse.Height / 2);
                                            break;
                                        case JointType.ElbowRight:
                                            ElbowRight = GetPosition2DLocation(depthImageFrame, joint.Position);
                                            ElbowRightDepth = joint.Position.Z;
                                            Canvas.SetLeft(rightElbowEllipse, ElbowRight.X - rightElbowEllipse.Width / 2);
                                            Canvas.SetTop(rightElbowEllipse, ElbowRight.Y - rightElbowEllipse.Height / 2);
                                            break;
                                    }
                                }


                                // Set line positions
                                leftLine.X1 = ElbowLeft.X;
                                leftLine.Y1 = ElbowLeft.Y;

                                leftLine.X2 = HandLeft.X;
                                leftLine.Y2 = HandLeft.Y;

                                rightLine.X1 = ElbowRight.X;
                                rightLine.Y1 = ElbowRight.Y;

                                rightLine.X2 = HandRight.X;
                                rightLine.Y2 = HandRight.Y;


                                if (!canvas.Children.Contains(leftHandFullBodyEllipse))
                                    canvas.Children.Add(leftHandFullBodyEllipse);

                                if (!canvas.Children.Contains(rightHandFullBodyEllipse))
                                    canvas.Children.Add(rightHandFullBodyEllipse);

                                if (!canvas.Children.Contains(leftElbowEllipse))
                                    canvas.Children.Add(leftElbowEllipse);

                                if (!canvas.Children.Contains(rightLine))
                                    canvas.Children.Add(rightLine);

                                if (!canvas.Children.Contains(leftLine))
                                    canvas.Children.Add(leftLine);

                                if (!canvas.Children.Contains(rightElbowEllipse))
                                    canvas.Children.Add(rightElbowEllipse);

                                //if (!leftHandCanvas.Children.Contains(leftHandEllipse))
                                //    leftHandCanvas.Children.Add(leftHandEllipse);

                                //if (!canvas.Children.Contains(rightHandEllipse))
                                //canvas.Children.Add(rightHandEllipse);

                                DepthCalculations(depthImageFrame);
                            }

                        }
                    }
                }
            }
        }

        WriteableBitmap wholeBodyBitmap;
        WriteableBitmap leftHandBitmap;
        short[] depthFrame;
        DepthImageFormat lastImageFormat;
        byte[] depthFrame32;

        void DepthCalculations(DepthImageFrame depthImageFrame)
        {
            // We need to detect if the format has changed.
            bool haveNewFormat = lastImageFormat != depthImageFrame.Format;

            if (haveNewFormat)
            {
                depthFrame = new short[depthImageFrame.PixelDataLength];
                depthFrame32 = new byte[depthImageFrame.Width * depthImageFrame.Height * Bgr32BytesPerPixel];
            }

            //byte[] convertedDepthBits = wholeBodyDepthFrame(depthImageFrame);

            int leftX = (int)HandLeft.X - 75;
            int leftY = (int)HandLeft.Y - 75;

            // A WriteableBitmap is a WPF construct that enables resetting the Bits of the image.
            // This is more efficient than creating a new Bitmap every frame.
            if (haveNewFormat)
            {
                //wholeBodyBitmap = new WriteableBitmap(depthImageFrame.Width, depthImageFrame.Height, 96, 96, PixelFormats.Bgr32, null);
                //fullDepthImage.Source = this.wholeBodyBitmap;

                //leftHandBitmap = new WriteableBitmap(depthImageFrame.Width, depthImageFrame.Height, 96, 96, PixelFormats.Bgr32, null);
                //depthLeftHand.Source = new CroppedBitmap(wholeBodyBitmap,
                //                                         new Int32Rect(
                //                                        (leftX < 0) ? 0 : leftX,
                //                                        (leftY < 0) ? 0 : leftY,
                //                                        (leftX + 50 >= depthImageFrame.Width) ? depthImageFrame.Width - leftX : 50,
                //                                        (leftY + 50 >= depthImageFrame.Height) ? depthImageFrame.Height - leftY : 50
                //                                        )
                //    );
            }

            //wholeBodyBitmap.WritePixels(new Int32Rect(0, 0, depthImageFrame.Width, depthImageFrame.Height), convertedDepthBits, depthImageFrame.Width * Bgr32BytesPerPixel, 0);

            depthLeftHand.Source = new CroppedBitmap(BitmapSource.Create(depthImageFrame.Width, depthImageFrame.Height, 192, 192, PixelFormats.Bgr32, null, LeftHandDepthFrame(depthImageFrame), depthImageFrame.Width * Bgr32BytesPerPixel),
                                                         new Int32Rect(
                                                        (leftX < 0) ? 0 : leftX,
                                                        (leftY < 0) ? 0 : leftY,
                                                        (leftX + 75 >= depthImageFrame.Width) ? depthImageFrame.Width - leftX : 75,
                                                        (leftY + 75 >= depthImageFrame.Height) ? depthImageFrame.Height - leftY : 75
                                                        )
                    );

            lastImageFormat = depthImageFrame.Format;

        }

        //void DepthCalculations(DepthImageFrame depthImageFrame)
        //{
        //    if (!tracked)
        //        return;

        //    depthImageFrame.

        //    fullDepthImage.Source = BitmapSource.Create(Image.Width, Image.Height, 96, 96, PixelFormats.Bgr32, null, wholeBodyDepthFrame(Image), Image.Width * 4);

        //    try
        //    {
        //        if (HandLeft.X == 0) { return; }
                
        //        double depthLeftHandModifier = (HandLeftDepth-1000) /10;

        //        int LeftHandX = (int)(HandLeft.X - 1.5 * (50 - .3 * depthLeftHandModifier));
        //        int LeftHandY = (int)(HandLeft.Y - 1.5 * (50 - .3 * depthLeftHandModifier));

        //        depthLeftHand.Source = new CroppedBitmap(BitmapSource.Create(Image.Width, Image.Height, 192, 192, PixelFormats.Bgr32, null, leftHandDepthFrame(Image), Image.Width * PixelFormats.Bgr32.BitsPerPixel / 8),
        //                             new Int32Rect(
        //                                           (LeftHandX < 0) ? 0 : (LeftHandX + (int)(1.5 * (75 - .6 * depthLeftHandModifier)) >= Image.Width) ? Image.Width - (int)(1.5 * (75 - .6 * depthLeftHandModifier)) : LeftHandX, //(LeftHandX < 0) ? 0 : LeftHandX,
        //                                           (LeftHandY < 0) ? 0 : (LeftHandY + (int)(1.5 * (75 - .6 * depthLeftHandModifier)) >= Image.Height) ? Image.Height - (int)(1.5 * (75 - .6 * depthLeftHandModifier)) : LeftHandY, //(LeftHandY < 0) ? 0 : LeftHandY,
        //                                           (int)(1.5 * (75 - .6 * depthLeftHandModifier)),//((int)(1.5 * (75 - .6 * depthLeftHandModifier)) >= depthLeftHand.Width) ? (int)depthLeftHand.Width - LeftHandX : (int)(1.5 * (75 - .6 * depthLeftHandModifier)),
        //                                           (int)(1.5 * (75 - .6 * depthLeftHandModifier))//((int)(1.5 * (75 - .6 * depthLeftHandModifier)) >= depthLeftHand.Height) ? (int)depthLeftHand.Height - LeftHandY : (int)(1.5 * (75 - .6 * depthLeftHandModifier))
        //                                          )
        //                             );

        //        borderLeft.Background = (isMakingAFist(depthLeftHand.Source)) ? Brushes.Red : Brushes.White;

                
        //        int depthRightHandModifier = (HandRightDepth - 1000) / 10;

        //        int RightHandX = (int)(HandRight.X - 1.5 * (75 - .3 * depthRightHandModifier));
        //        int RightHandY = (int)(HandRight.Y - 1.5 * (60 - .3 * depthRightHandModifier));

        //        depthRightHand.Source = new CroppedBitmap(BitmapSource.Create(Image.Width, Image.Height, 192, 192, PixelFormats.Bgr32, null, rightHandDepthFrame(Image), Image.Width * 4),
        //                             new Int32Rect(
        //                                           (RightHandX < 0) ? 0 : (RightHandX + (int)(1.5 * (75 - .6 * depthRightHandModifier)) >= Image.Width) ? Image.Width - (int)(1.5 * (75 - .6 * depthRightHandModifier)) : RightHandX,
        //                                           (RightHandY < 0) ? 0 : (RightHandY + (int)(1.5 * (75 - .6 * depthRightHandModifier)) >= Image.Height) ? Image.Height - (int)(1.5 * (75 - .6 * depthRightHandModifier)) : RightHandY,
        //                                           (int)(1.5 * (75 - .6 * depthRightHandModifier)),//(RightHandX + (int)(1.5 * (75 - .6 * depthRightHandModifier)) >= Image.Width) ? Image.Width - RightHandX : (int)(1.5 * (75 - .6 * depthRightHandModifier)),
        //                                           (int)(1.5 * (75 - .6 * depthRightHandModifier))//(RightHandY + (int)(1.5 * (75 - .6 * depthRightHandModifier)) >= Image.Height) ? Image.Height - RightHandY : (int)(1.5 * (75 - .6 * depthRightHandModifier))
        //                                          )
        //                             );

        //        borderRight.Background = (isMakingAFist(depthRightHand.Source)) ? Brushes.Red : Brushes.White;
        //    }
        //    catch(Exception ex)
        //    { }
        //}

        int getValue(DepthImageFrame imageFrame, int x, int y)
        {
            short[] pixelData = new short[imageFrame.PixelDataLength];
            imageFrame.CopyPixelDataTo(pixelData);
            return ((ushort)
              pixelData[x + y * imageFrame.Width]) >> 3;
        }

        private Point GetPosition2DLocation(DepthImageFrame depthFrame, SkeletonPoint skeletonPoint)
        {
            ImageType ImageType = ImageType.Depth;

            DepthImagePoint depthPoint = depthFrame.MapFromSkeletonPoint(skeletonPoint);

            switch (ImageType)
            {
                case ImageType.Color:
                    ColorImagePoint colorPoint = depthFrame.MapToColorImagePoint(depthPoint.X, depthPoint.Y, kinectSensor.ColorStream.Format);

                    // map back to skeleton.Width & skeleton.Height
                    return new Point(
                        (int)(this.RenderSize.Width * colorPoint.X / kinectSensor.ColorStream.FrameWidth),
                        (int)(this.RenderSize.Height * colorPoint.Y / kinectSensor.ColorStream.FrameHeight));
                case ImageType.Depth:
                    double depthX = Math.Max(0, Math.Min(fullDepthImage.Width * depthPoint.X / depthFrame.Width, fullDepthImage.Width));
                    double depthY = Math.Max(0, Math.Min(fullDepthImage.Height * depthPoint.Y / depthFrame.Height, fullDepthImage.Height));
                    return new Point((int)depthX, (int)depthY);
                default:
                    throw new ArgumentOutOfRangeException("ImageType was a not expected value: " + ImageType.ToString());
            }
        }

        // Out depth is in millimeters
        //private Point getDisplayPosition(Joint joint, out short depth)
        //{
        //    int colourX, colourY;
        //    float depthX, depthY;
        //    this.nui.SkeletonEngine.SkeletonToDepthImage(joint.Position, out depthX, out depthY, out depth);

        //    depth = (short)(depth >> 3);

        //    depthX = Math.Max(0, Math.Min(depthX * 320, 320));
        //    depthY = Math.Max(0, Math.Min(depthY * 240, 240));
        //    ImageViewArea imageView = new ImageViewArea();
        //    this.nui.NuiCamera.GetColorPixelCoordinatesFromDepthPixel(ImageResolution.Resolution640x480, imageView, (int)depthX, (int)depthY, 0, out colourX, out colourY);
        //    return new Point((int)(fullDepthImage.Width * colourX / 640.0), (int)(fullDepthImage.Height * colourY / 480));
        //}

        bool isMakingAFist(ImageSource imageSource)
        {
            int gray = 255 / 2;
            bool wasGray = false;
            int GrayWidth = 0;
            int GrayTimes = 0;

            for (int y = 10; y < imageSource.Height - 10; y += 10)
            {
                for (int x = 3; x < imageSource.Width; x++)
                {
                    CroppedBitmap bitmap = new CroppedBitmap((BitmapSource)imageSource, new Int32Rect(x,y,1,1));
                    byte[] pixel = new byte[4];
                    bitmap.CopyPixels(pixel, 4, 0);

                    if (pixel[2] == gray && pixel[1] == gray && pixel[0] == gray)
                    {
                        if (!wasGray)
                        {
                            if (GrayWidth > 1 && GrayWidth < 15)
                            { GrayTimes++; }
                            GrayWidth = 0;
                        }
                        else
                        {
                            GrayWidth++;
                        }

                        wasGray = true;
                    }
                    else
                    {
                        wasGray = false;
                    }

                }
                if (GrayTimes > 1) { return false; }
            }

            return true;
        }

        private void GetWidth(ImageSource imageSource)
        {
            int gray = 255 / 2;
            bool firstY = false;
            Point firstYPoint;

            for(int y = 10; y < imageSource.Height - 10; y+=10)
            {
                for(int x = 3; x < imageSource.Width; x++)
                {
                    CroppedBitmap bitmap = new CroppedBitmap((BitmapSource)imageSource, new Int32Rect(x,y,1,1));
                    byte[] pixel = new byte[4];
                    bitmap.CopyPixels(pixel, 4, 0);

                    if (pixel[2] == gray && pixel[1] == gray && pixel[0] == gray)
                    {
                        if (!firstY)
                            firstYPoint = new Point(x, y);
                    }
                    else
                    {
                        
                    }
                  
                }

            }
        }
        
        
        byte[] wholeBodyDepthFrame(DepthImageFrame depthImageFrame)
        {
            //int tooNearDepth = 1000;
            //int tooFarDepth = 2000;

            depthImageFrame.CopyPixelDataTo(depthFrame);
            
            for (int i16 = 0, i32 = 0; i16 < depthFrame.Length && i32 < depthFrame32.Length; i16++, i32 += 4)
            {
                int player = depthFrame[i16] & DepthImageFrame.PlayerIndexBitmask;
                int realDepth = depthFrame[i16] >> DepthImageFrame.PlayerIndexBitmaskWidth;
                
                // transform 13-bit depth information into an 8-bit intensity appropriate
                // for display (we disregard information in most significant bit)
                byte intensity = (byte)(~(realDepth >> 4));

                if (player == 0)
                {
                    depthFrame32[i32 + RedIndex] = 255;
                    depthFrame32[i32 + GreenIndex] = 255;
                    depthFrame32[i32 + BlueIndex] = 255;
                }
                else
                {
                    // tint the intensity by dividing by per-player values
                    depthFrame32[i32 + RedIndex] = 0;//(byte)(intensity >> IntensityShiftByPlayerR[player]);
                    depthFrame32[i32 + GreenIndex] = 255;//(byte)(intensity >> IntensityShiftByPlayerG[player]);
                    depthFrame32[i32 + BlueIndex] = 0;//(byte)(intensity >> IntensityShiftByPlayerB[player]);
                }
            }

            return depthFrame32;
        }

        byte[] LeftHandDepthFrame(DepthImageFrame depthImageFrame)
        {
            //int tooNearDepth = 1000;
            //int tooFarDepth = 2000;

            depthImageFrame.CopyPixelDataTo(depthFrame);

            int depth = (int)(HandLeftDepth * 1000 + 100);

            for (int i16 = 0, i32 = 0; i16 < depthFrame.Length && i32 < depthFrame32.Length; i16++, i32 += 4)
            {
                int player = depthFrame[i16] & DepthImageFrame.PlayerIndexBitmask;
                int realDepth = depthFrame[i16] >> DepthImageFrame.PlayerIndexBitmaskWidth;

                // transform 13-bit depth information into an 8-bit intensity appropriate
                // for display (we disregard information in most significant bit)
                byte intensity = (byte)(~(realDepth >> 4));

                if (player == 0)
                {
                    depthFrame32[i32 + RedIndex] = 255;
                    depthFrame32[i32 + GreenIndex] = 255;
                    depthFrame32[i32 + BlueIndex] = 255;
                }
                else if (depth > realDepth && depth < realDepth + 50)
                {
                    // tint the intensity by dividing by per-player values
                    depthFrame32[i32 + RedIndex] = 0;//(byte)(intensity >> IntensityShiftByPlayerR[player]);
                    depthFrame32[i32 + GreenIndex] = 255;//(byte)(intensity >> IntensityShiftByPlayerG[player]);
                    depthFrame32[i32 + BlueIndex] = 0;//(byte)(intensity >> IntensityShiftByPlayerB[player]);
                }
            }

            return depthFrame32;
        }

        int getDepth(short[] pixelData, int width, int x, int y)
        {
            return ((ushort)pixelData[x + y * width]) >> 3;
        }

        //byte[] leftHandDepthFrame(PlanarImage image)
        //{
        //    byte[] depthFrame16 = image.Bits;
        //    byte[] depthFrame32 = new byte[image.Height * image.Width * 4];

        //    for (int i16 = 0, i32 = 0; i16 < depthFrame16.Length && i32 < depthFrame32.Length; i16 += 2, i32 += 4)
        //    {
        //        int realDepth = (depthFrame16[i16 + 1] << 5) | (depthFrame16[i16] >> 3);

        //        byte intensity = (byte)(255); // (255 - (255 * realDepth / 0x0fff));

        //        int depth = (HandLeftDepth);

        //        if (realDepth < depth + 130 && realDepth > depth - 150)
        //        {
        //             Gray
        //            depthFrame32[i32 + RED_IDX] = (byte)(intensity / 2);
        //            depthFrame32[i32 + GREEN_IDX] = (byte)(intensity / 2);
        //            depthFrame32[i32 + BLUE_IDX] = (byte)(intensity / 2);
        //        }
        //        else
        //        {
        //             White
        //            depthFrame32[i32 + RED_IDX] = 255;
        //            depthFrame32[i32 + GREEN_IDX] = 255;
        //            depthFrame32[i32 + BLUE_IDX] = 255;
        //        }
        //    }
        //    return depthFrame32;
        //}

        //byte[] rightHandDepthFrame(PlanarImage image)
        //{
        //    byte[] depthFrame16 = image.Bits;
        //    byte[] depthFrame32 = new byte[image.Height * image.Width * 4];

        //    for (int i16 = 0, i32 = 0; i16 < depthFrame16.Length && i32 < depthFrame32.Length; i16 += 2, i32 += 4)
        //    {
        //        int realDepth = (depthFrame16[i16 + 1] << 5) | (depthFrame16[i16] >> 3);

        //        byte intensity = (byte)(255); // (255 - (255 * realDepth / 0x0fff)); //Displays only black

        //        int depth = (HandRightDepth);

        //        if (realDepth < depth + 130 && realDepth > depth - 150)
        //        {
        //             Gray
        //            depthFrame32[i32 + RED_IDX] = (byte)(intensity / 2);
        //            depthFrame32[i32 + GREEN_IDX] = (byte)(intensity / 2);
        //            depthFrame32[i32 + BLUE_IDX] = (byte)(intensity / 2);
        //        }
        //        else
        //        {
        //             White
        //            depthFrame32[i32 + RED_IDX] = 255;
        //            depthFrame32[i32 + GREEN_IDX] = 255;
        //            depthFrame32[i32 + BLUE_IDX] = 255;
        //        }
        //    }
        //    return depthFrame32;
        //}

        private void adjustElevationClick(object sender, RoutedEventArgs e)
        {
            int i;
            bool CanBeInt = int.TryParse(txtAngle.Text, out i);
            if (CanBeInt && i > -28 && i < 28) { kinectSensor.ElevationAngle = i; }
        }
    }


}
